﻿#ifndef NAVIGATOR_H
#define NAVIGATOR_H

class Maindeal;
#include <QList>
#include <QObject>

#include "kalmanfilter.h"
#include "main.h"
#include "maindeal.h"
#include "param.h"
#include "pid.h"
#include "point.h"
#include "qnode.hpp"
#include "slamrouteplanning.h"
#include "station.h"
#include "utility.h"

class Navigator : public QObject {
    Q_OBJECT
public:
    explicit Navigator( int argc, char** argv, Maindeal* maindeal, QObject* parent = nullptr );
    ~Navigator();

    enum NavigationMode {
        NAVIGATION_NONE = 0,
        //    NAVIGATION_COOR,
        NAVIGATION_SLAM = 2,
    };
    Q_ENUM( NavigationMode )

    enum SlamNaviStatus {
        NONE_SPEED          = -3,
        NONE_ROUTE          = -2,
        ERROR               = -1,
        NOMAL               = 0,
        CANNONT2TARGET      = 1,
        OBS_CANNONT2TARGET  = 2,
        STOP_CANNONT2TARGET = 3,
        AROUND_THE_OBS      = 4,
    };
    Q_ENUM( SlamNaviStatus )

    enum CalcNextStationResult { WAIT_PREPARE, SUCCESSED, FAILED };
    Q_ENUM( CalcNextStationResult )

    enum NaviStep { READY, TRACING, ARRIVE, ADJUST, NAVI_ERROR };
    Q_ENUM( NaviStep )

    const QList< Station >& stations_list();
    Point                   curPoint() const;
    Point                   slamPoint() const;
    NaviStep                getSlamNaviStep() const;
    bool                    calcNextStation();
    bool                    calcCoordinateRoute();
    bool                    calcSlamRoute();
    bool                    execSlamRoute();
    void                    clearEmergencyState();
    void setAutoModeEnable( bool isAutoModeEnable );
    void setEmergencyStop( bool isEmergencyStop );
    void resetAllSwitchAndParam();
    void slamNaviStart();
    void EmergencyStop();
    void setSlamPose( const Point& point );
    void dealSlamNaviStep( const NaviStep& step );
    void changeObs();
    void getSlamNaviCurId();
    void getObsState( int& front, int& back );  //获取避障切换情况，用于界面显示
    void getSlamParam( double& Erad, double& Eh, double& Edis );
    double getSlamRouteLength();
    void lockLocalization( bool sw );
    bool isAutoModeEnable() const;
    bool isEmergencyStop() const;
    bool isForward() const;
    bool isObstacleFunctionEnable() const;

    bool SystemNaviOrder( int route, bool isAutoRoute );
    bool SystemNaviOrder2( int id, SlamRoutePlanning::RouteType route_type, bool isAutoRoute );
    bool SystemNaviOrder3( int start_id, int goal_id, SlamRoutePlanning::RouteType route_type, bool isAutoRoute );

    bool isAllowOpenCharge() const;
    bool isNearTargetStation() const;
    bool inOnRoute( const int& num, int& err );
    bool isAllowSetRoute();
    bool isAutoSetRoute();
    bool isInitPose();
    ////////////////////////////////////////
    double get_Fork_VeerSettingRad() const;
    double get_Fork_WalkSettingSpeed() const;
    double get_Fork_VeerRealityRad() const;
    double get_Fork_WalkRealitySpeed() const;
    void setForkWalksettingspeed( double value );
    void setForkVeersettingrad( double value );

    bool isOnRoutes( const QList< SlamRoutePlanning::StationModel >& station_models, int* index = nullptr, double* dist = nullptr, double* dth = nullptr );
    bool isOnRoute( const SlamRoutePlanning::StationModel& station_model, double* dist = nullptr, double* dth = nullptr );

    void               getCanNaviParam();
    void               calcPosition();
    int                getSlamRoute() const;
    int                getSlamId() const;
    int                getSlamStartId() const;  //加入
    int                getSlamGoalId() const;
    int                getSlamCurProgress() const;
    int                getSlamTagProgress() const;
    NavigationMode     getNavigationMode() const;
    QString            showRoute() const;
    SlamRoutePlanning* slamRoutePlanning() const;
    qnode::QNode*      qnodeModule() const;
    SlamNaviStatus     getSlamNaviStatus() const;
    //日志中文显示
    QString getAgvStateChinese();
    QString getNavigationModeChinese( Navigator::NavigationMode num );
    QString getSlamNaviStatusChinese( Navigator::SlamNaviStatus num );

private:
    NavigationMode     navigation_mode     = NAVIGATION_NONE;  //导航模式的选择
    Maindeal*          maindeal            = nullptr;
    SlamRoutePlanning* slam_route_palnning = nullptr;
    qnode::QNode*      qnode_module        = nullptr;

    int    argc;
    char** argv;
    bool   is_auto_mode_enable         = false;  //开启自动模式的开关
    bool   is_obstacle_function_enable = true;   //控制避障是否开启的开关.默认状态为开,应在对接设备时关闭.
    bool   is_obstacle_slowdown_enable = true;   //控制避障中减速区域的使能，默认开启
    bool   is_emergency_stop = false;  // AGV中对异常是一直刷新检查的，可以在没恢复正常的时候一直报警。但是对于导航中的错误触发只有一次，所以要使用bool的变量。
    bool   is_qrcode_new_value = false;  //新接收二维码数据的判断

    bool is_need_redo_slam_route = false;  //是否需要激光路线重对接

    //  bool is_slam_navi_last = false;        //判断是否是激光导航最后一段，且是要允许传输

    bool is_slam_need_to_stop     = false;  //激光导航中的路线段是否需要停障。
    bool is_auto_set_route        = false;  //是否自动设定路线
    bool is_init_pose_done        = false;  //是否重定位锁地图
    bool is_first_time_arrive_sig = false;  //是否第一次收到ROS的到达的信号
    int  turn_dis                 = 500;    //弧形转弯时候选择的距离

    double dis_err_limit               = 300.0;
    double slam_point_to_line_distance = 0.0;
    double slam_rad_err_start2target   = 0.0;
    double slam_dis_err_cur2target     = 0.0;
    double slam_dis_start2target       = 0.0;
    //  int qrcode_tag_id = 0;                  //现在的二维码标签id
    //  int coor_cur_navi_progress = 0;         //坐标导航的进度
    //  int coor_cur_navi_target_progress = 0;  //坐标导航的目标进度
    int slam_cur_navi_route           = 0;      //激光导航的路线
    int slam_cur_navi_id              = 99999;  //激光导航id
    int slam_start_navi_id            = 99999;  //
    int slam_cur_navi_progress        = 0;      //激光导航的进度
    int slam_cur_navi_target_progress = 0;      //激光导航的目标进度
    int front_obs                     = 3;      //存避障切换情况，用于界面显示
    int back_obs                      = 3;

    QList< SlamRoutePlanning::StationModel > current_station_models{};

    struct {
        int                          start_id;
        int                          goal_id;
        SlamRoutePlanning::RouteType route_type;
    } SystemNaviOrder3_config;

    ///////////////////////////////////////////////////////////////
    double fork_walk_setting_speed = 0;  // m/s
    double fork_veer_setting_rad   = 0;  // rad

    double fork_walk_reality_speed = 0;  // m/s
    double fork_veer_reality_rad   = 0;  // rad

    double slam_walkspeed = 0;  //叉车舵轮直行速度 单位cm/s
    double slam_veerrad   = 0;  //叉车舵轮转角速度 单位cm/s

    Point cur_point;  //当前坐标,各种数据融合后的坐标
    Point slam_point;
    //  QList<Station> stations;  //路线中的站点集合
    SlamRoutePlanning::StationModel sub_route;  ///<当前子路线
    //  bool is_forward = true;            // AGV行驶的方向

    QString        cur_machine;
    NaviStep       slamNaviStep     = ADJUST;  //写其他会有bug，必须是一个没用的默认状态
    SlamNaviStatus slam_navi_status = NOMAL;
    QTimer         qnode_connect_time;
signals:
    void sigSendTips( const QString& );

public slots:
    void sltNavigatorInit();
    void sltStartButtonHit();
    void slt50msTimer();
    void sltRosShutdown();
    void sltcurrentPose( const QList< double >& pose );
    void slttargetSpeed( const QList< double >& speed );
    void sltonGoalReached();
    void sltonNavStatus( const int& state );
    void sigonLocalizationLose();
    void sigLog();
};

#endif  // NAVIGATOR_H
